#include "Grid.h"
#include <queue>
#include "NodePriQ.h"
#include "Engine.h"
#include "LineDrawer.h"

Grid::Grid() : 
m_quater0(0),
m_quater1(0),
m_quater2(0),
m_quater3(0)
{
}
Grid::~Grid()
{
    if(m_quater0)
        delete[] m_quater0;
    if(m_quater1)
        delete[] m_quater1;
    if(m_quater2)
        delete[] m_quater2;
    if(m_quater3)
        delete[] m_quater3;
}
void Grid::CreateNodes(int numX, int numY, float distX, float distY, const D3DXVECTOR3& startPos)
{
    m_gridSizeX = numX;
    m_gridSizeY = numY;
    m_nodeDistanceX = distX;
    m_nodeDistanceY = distY;
    float startX = startPos.x - (m_gridSizeX/2) * m_nodeDistanceX;
    float startY = startPos.z + (m_gridSizeY/2) * m_nodeDistanceY;
    if(numX % 2 == 0)
        startX += distX / 2;
    if(numY % 2 == 0)
        startY -= distY / 2;
    float currentX = startX;
    float currentY = startY;
    float stepX = m_nodeDistanceX;
    float stepY = m_nodeDistanceY;

    //These are the mid way points for the comparison later on
    float tDistX = startX + (0.5f * m_gridSizeX * m_nodeDistanceX);
    float tDistY = startY - (0.5f * m_gridSizeY * m_nodeDistanceY);
    int amount = m_gridSizeX*m_gridSizeY/4;
    m_quater0 = new int[amount];
    m_quater1 = new int[amount];
    m_quater2 = new int[amount];
    m_quater3 = new int[amount];
    int indexs[4] = {0,0,0,0};
    for(int i = 0; i < m_gridSizeX; ++i)
    {  
        for(int j = 0; j < m_gridSizeY; j++)
        {
            GraphNode n;
            n.SetId(i * m_gridSizeX + j);
            n.SetPositon(D3DXVECTOR3(currentX,0,currentY));
            currentX += stepX;
            m_graph.AddNode(n);
            if(currentX <= tDistX && currentY > tDistY)
            {
                //we are top left
                int temp = i * m_gridSizeX + j;
                m_quater0[indexs[0]] = temp;
                indexs[0]++;
            }
            else if(currentX <= tDistX && currentY <= tDistY)
            {
                //we are bottom left
                int temp = i * m_gridSizeX + j;
                m_quater1[indexs[1]] = temp;
                indexs[1]++;
            }
            else if(currentX > tDistX && currentY > tDistY)
            {
                //we are top right
                int temp = i * m_gridSizeX + j;
                m_quater2[indexs[2]] = temp;
                indexs[2]++;
            }
            else if(currentX >= tDistX && currentY <= tDistY)
            {
                //we are bottom right
                int temp = i * m_gridSizeX + j;
                m_quater3[indexs[3]] = temp;
                indexs[3]++;
            }
        }    
        currentX = startX;
        currentY -= stepY;
    }
    int test = 0;
}
void Grid::CreateEdges(bool diag)
{
    int totalSize = m_gridSizeX * m_gridSizeY;
    for(int i = 0; i < totalSize; ++i)
    {
        GraphEdge e;
        if( (i+1) % m_gridSizeX > 0) //to the right
        {
            e.From(i);
            e.To(i+1);
            e.Cost(m_nodeDistanceX);
            m_graph.AddEdge(e);
        }
        if(i+m_gridSizeX < totalSize) // below
        {
            e.From(i);
            e.To(i+m_gridSizeX);
            e.Cost(m_nodeDistanceY);
            m_graph.AddEdge(e);
        }
        if(i % m_gridSizeX != 0 && i -1 >= 0) // to the left
        {
            e.From(i);
            e.To(i-1);
            e.Cost(m_nodeDistanceX);
            m_graph.AddEdge(e);
        }
        if(i-m_gridSizeX >= 0) // above
        {
            e.From(i);
            e.To(i-m_gridSizeX);
            e.Cost(m_nodeDistanceY);
            m_graph.AddEdge(e);
        }
    }
    if(diag)
    {
        float diagionalCost = sqrt(m_nodeDistanceX*m_nodeDistanceX +
                                    m_nodeDistanceY*m_nodeDistanceY );
        GraphEdge e;
        for(int i = 0; i < totalSize; ++i)
        {
            //Bottom RIGHT
            if((i+1) % m_gridSizeX != 0 &&       // if we are not on the far right,
                i + (m_gridSizeX+1) < totalSize) // if we are not the bottom right corner
            {
                e.From(i);
                e.To(i+(m_gridSizeX+1));
                e.Cost(diagionalCost);
                m_graph.AddEdge(e);
            }

            //Bottom Left
            if((i) % m_gridSizeX != 0 &&          // if we are not the far left 
                i+m_gridSizeX-1 < totalSize)    // if we are not on the bottom left corner
            {
                e.From(i);
                e.To(i+(m_gridSizeX - 1));
                e.Cost(diagionalCost);
                m_graph.AddEdge(e);
            }

            // top left
            if( i % m_gridSizeX != 0 &&    //If we are not on the left side
                i -(m_gridSizeX + 1) >= 0) //if we are not the top row
            {
                e.From(i);
                e.To(i-(m_gridSizeX+1));
                e.Cost(diagionalCost);
                m_graph.AddEdge(e);
            }

            //top right 
            if((i+1) % m_gridSizeX != 0 &&      //if we are not on the far right 
                i-(m_gridSizeX-1) >=0)          //and we are not on the top row 
            {
                e.From(i);
                e.To(i-m_gridSizeX+1);
                e.Cost(diagionalCost);
                m_graph.AddEdge(e);
            }
        }
    }
}
void Grid::DrawNodes(ID3DXEffect* effect) const
{
    D3DXVECTOR3 black(0,0,0);
    D3DXVECTOR3 red(1,0,0);
    
    for(int i = 0; i < m_gridSizeX*m_gridSizeY; ++i)
    {
        D3DXVECTOR3 col;
        if(m_graph.Nodes()[i].GetID() == -1)
            col = black;
        else
            col = red;

        Engine::Instance()->GetLineDrawer()->drawLine(m_graph.Nodes()[i].GetPos(),
                                                        m_graph.Nodes()[i].GetPos() + m_nodeDrawDirection,
                                                    col);
    }
}
void Grid::DrawEdge(int start, int finish, const D3DXVECTOR3& color ) const
{
    Engine::Instance()->GetLineDrawer()->drawLine(m_graph.Nodes()[start].GetPos() + m_nodeDrawDirection,
                               m_graph.Nodes()[finish].GetPos() + m_nodeDrawDirection,
                               color);
}
void Grid::DrawEdges(ID3DXEffect* effect) const
{
    for(unsigned int i = 0; i < m_graph.ListEdges().size(); ++i)
    {
        for(lEdge::const_iterator it = m_graph.ListEdges()[i].begin();
            it != m_graph.ListEdges()[i].end();
            ++it)
        {
            DrawEdge(it->From(), it->To(), D3DXVECTOR3(1,0,1));
        }
    }
}
void Grid::SetNodeDrawSize( const D3DXVECTOR3& size )
{
    m_nodeDrawDirection = size;
}
bool Grid::PathFindDepthFirst(int start, int finish,std::list<int>* path) const
{
    /* Depth first */

    //every node will have a parent that is closest to the start node.
    std::vector<int> parents(m_graph.GetCurrentIndex(), -1);
    //every node will be visited, or not.
    std::vector<bool> visited(m_graph.GetCurrentIndex(), false);
    //Last in, first out, we test the newest edges first.
    std::stack<GraphEdge> edges;    
    //make dummy edge from start to start,
    GraphEdge e;
    e.From(start);
    e.To(start);
    edges.push(e);
    //while stack is not empty,
    while(!edges.empty())
    {
        //get the top edge,
        GraphEdge current = edges.top();
        edges.pop();
        //The node this edge is going TO, gets its parent as the node this edge is commign FROM
        parents[current.To()] = current.From();
        //MARK current node AS VISITED
        visited[current.From()] = true;
        //check if the to is our target
        if(current.To() == finish)
        {
            //if it is, we need to loop over the parents list,
            //and essentially back track, filling up the path* passed in.

            //start with the final node
            int i = finish;
            while(i != start)
            {
                //if were not at the start yet, add it to the path, and
                //now look atht eh child node
                path->push_back(i);
                i = parents[i];
            }
            //finally, add the start node, just to keep things tidy
            path->push_back(start);
            return true;
        }
        // if not, add all edges to the stack if the to node hasnt been visited
        for(lEdge::const_iterator it = m_graph.ListEdges()[current.To()].begin();
            it != m_graph.ListEdges()[current.To()].end();
            ++it)
        {
            if(!visited[it->To()])
                edges.push(*it);
        }
    }     
    return false;
};
bool Grid::PathFindBreadthFirst(int start, int finish, std::list<int>* path)  const
{
    /* Breadth first */

    //Same method as breadth.
    std::vector<int> parents(m_graph.GetCurrentIndex(), -1);
    std::vector<bool> visited(m_graph.GetCurrentIndex(), false);
    std::queue<GraphEdge> edges;    
    //make dummy edge from start to start,
    GraphEdge e;
    e.From(start);
    e.To(start);
    edges.push(e);
    //while stack is not empty,
    while(!edges.empty())
    {
        //get the top edge,
        GraphEdge current = edges.front();
        edges.pop();
        //NOTE PARENT of the TO node
        parents[current.To()] = current.From();
        //MARK current node AS VISITED
        visited[current.From()] = true;
        //check if the to is our target
        if(current.To() == finish)
        {
            int i = finish;
            while(i != start)
            {
                path->push_back(i);
                i = parents[i];
            }
            path->push_back(start);
            return true;
        }
        // if not, add all edges to the stack if the to node hasnt been visited
        for(lEdge::const_iterator it = m_graph.ListEdges()[current.To()].begin();
            it != m_graph.ListEdges()[current.To()].end();
            ++it)
        {
            if(!visited[it->To()])
            {
                edges.push(*it);
                /************************************************************************/
                /* This is the only difference, when we add the edge to the stack, we mark
                   it's from ndoe as visited as soon as we add it to the stack          */
                /************************************************************************/
                visited[it->From()] = true;
            }
        }
    }     
    return false;
};
bool Grid::DijkstraSeach(int start, int finish, std::list<int>* path)  const
{
    /************************************************************************/
    /*  This method finds the shortest path to the START node for ALL OTHER nodes.
        
        It is obviously very innefficient, as usually we only care about two nodes.

        It does have its uses, if you imagine the two nodes forming the center and
        radius of a circle, this test will find the shortes path to the center for 
        ALL nodes in the circle.                                        
        
        We use a custom priority queue class, that does its comparing via an external
        COST vector. Each node will share the same index into the vector.
        
        COST will store the current lowest value way of getting to that node. */
    /************************************************************************/

    //we need to size all the vectors to stop index failing.
    int size = m_graph.Nodes().size();

    //OK This is a vector of edges, that for each node,
    //represent the edges we haven't explored yet
    std::vector<GraphEdge> openEdges(size, GraphEdge());
    //This vector represents the current LOWEST COST edge back to the source.
    //closed edges wil eventually hold the path from Finish to Start,
    //but it will also hold the path from ANY node to the start node.
    std::vector<GraphEdge> closedEdges(size, GraphEdge());

    //This is the external vector of costs that we manage on a per node basis.
    std::vector<float> costs(size, 0);

    //This is a priority que that works exclusivly from values in an external data structure
    NodePriQ pq(costs);
    //create dummy edge from start to start, with a cost of 0;
    GraphEdge dummy;
    dummy.From(start); dummy.To(start);
    dummy.Cost(0);
    //we add this edge to the open list, as we are looking at it first
    openEdges[start] = dummy;
    //insert it into the pri-queu
    pq.Insert(start);
    //handy to do this here, as edges arent likely to change during the search
    vEdgeList edges = m_graph.ListEdges();

    while(pq.Size() != 0)
    {
        //get the FRONT edge (the one with the lowest cost)
        int index = pq.Front();
        pq.Pop();
        //mark it as closed, storee the shortest path
        closedEdges[index] = openEdges[index];

        //if we have found our node, construct the path and return true
        if(index == finish)
        {
            int num = finish;
            path->push_back(num);
            while(num != start)
            {
                path->push_back(closedEdges[num].From());
                num = closedEdges[num].From();
            }
            pq.Clear();
            return true;
        }

        //other whys, get the list of edges going FROM the current node
        lEdge currentEdges = edges[index];
        // for each edge in current edges,
        for(lEdge::iterator it = currentEdges.begin(); it != currentEdges.end(); ++it)
        {
            //calc cost for getting to this node,
            float total = costs[index] + it->Cost();   //the cost is the cost of the edge, 
                                                        //pluss the running total to get to this node
            //if this edge is not on the open list, we have never seen it before
            if(openEdges[it->To()].From() == -1 ||
               openEdges[it->To()].To()   == -1)
            {
                //set the cost in our vector,
                costs[it->To()] = total;
                //this is the current shortest path to it->TO()
                openEdges[it->To()] = *it;
                pq.Insert(it->To());
            }
            else if(closedEdges[it->To()].From() == -1 && // if we have seen it,
                    closedEdges[it->To()].To()   == -1 &&
                    costs[it->To()] > total)  // and this is shorter
            {
                //update the costs, edit the OPEN edge
                costs[it->To()] = total;
                openEdges[it->To()] = *it;
                //resort the queu, as this might be the newest shortest path.
                pq.Resort();
            }       
        }
    }
    //clear the queu, return
    pq.Clear();
    return false;
}
float Grid::ManhattanHeuristic(int start, int finish) const
{
    D3DXVECTOR2 diff = m_graph.m_nodes[start].GetPos() - 
        m_graph.m_nodes[finish].GetPos();
    return abs(diff.x + diff.y);
}
float Grid::EucludianHeristic(int start, int finish) const
{
    D3DXVECTOR2 diff = m_graph.m_nodes[start].GetPos() - 
        m_graph.m_nodes[finish].GetPos();
    return D3DXVec2LengthSq(&diff);
}
bool Grid::AStarSearch(int start, int finish, std::list<int>* path)  const
{
    int size = m_graph.m_nodes.size();

    std::vector<GraphEdge> openEdges(size, GraphEdge());
    std::vector<GraphEdge> closedEdges(size, GraphEdge());

    std::vector<float> hCosts(size, 0);    //heuristic
    std::vector<float> gCosts(size, 0);    //accumulated cost, similar to dj
    std::vector<float> fCosts(size, 0);    //added versions of both
    //This is a priority que that works exclusivly from values in an external data structure
    NodePriQ pq(fCosts);
    //create dummy edge from start to start, with a cost of 0;
    GraphEdge dummy;
    dummy.From(start); dummy.To(start);
    dummy.Cost(0);
    //we add this edge to the open list, as we are looking at it first
    openEdges[start] = dummy;
    //insert it into the pri-queu
    pq.Insert(start);

    while(pq.Size() != 0)
    {
        //get the FRONT edge (the one with the lowest cost)
        int index = pq.Front();
        pq.Pop();
        //mark it as closed, storee the shortest path
        closedEdges[index] = openEdges[index];

        //if we have found our node, construct the path and return true
        if(index == finish)
        {
            int num = finish;
            path->push_back(num);
            while(num != start)
            {
                path->push_back(closedEdges[num].From());
                num = closedEdges[num].From();
            }
            pq.Clear();
            return true;
        }

        //other whys, get the list of edges going FROM the current node
        const lEdge& currentEdges = m_graph.m_listEdges[index];
        // for each edge in current edges,
        for(lEdge::const_iterator it = currentEdges.begin(); it != currentEdges.end(); ++it)
        {
            //calc cost for getting to this node,
            float total = gCosts[index] + it->Cost();   //the cost is the cost of the edge, 
                                                        //pluss the running total to get to this node
            //if this edge is not on the open list, we have never seen it before
            if(openEdges[it->To()].From() == -1 ||
               openEdges[it->To()].To()   == -1)
            {
                //set the cost in our vector,
                gCosts[it->To()] = total;
                hCosts[it->To()] = ManhattanHeuristic(it->From(), it->To());
                fCosts[it->To()] = gCosts[it->To()] + hCosts[it->To()];
                //this is the current shortest path to it->TO()
                openEdges[it->To()] = *it;
                pq.Insert(it->To());
            }
            else if(closedEdges[it->To()].From() == -1 && // if we have seen it,
                    closedEdges[it->To()].To()   == -1 &&
                    gCosts[it->To()] > total)  // and this is shorter
            {
                //update the costs, edit the OPEN edge
                gCosts[it->To()] = total;
                fCosts[it->To()] = gCosts[it->To()] + hCosts[it->To()];
                openEdges[it->To()] = *it;
                //resort the queu, as this might be the newest shortest path.
                pq.Resort();
            }       
        }
    }
    //clear the queu, return
    pq.Clear();
    return false;
}
const Graph& Grid::GetGraph()  const
{
    return m_graph;
}
//This desperately needs a BVH of some sort,
int Grid::GetNearestNode(const D3DXVECTOR3& position, float sphereRad) const
{
    float minDistSquared = FLT_MAX;
    int currentIndex = 0;
    int* array = 0;
    float x = position.x;
    float y = position.z;
    if(x <= 0 && y > 0)
    {
        array = m_quater0;
    }
    else if(x <= 0 && y <= 0)
    {
        array = m_quater1;
    }
    else if(x > 0 && y > 0)
    {
       array = m_quater2;
    }
    else if(x >= 0 && y <= 0)
    {
        array = m_quater3;
    }
    for(int i = 0; i < m_gridSizeX*m_gridSizeY/4; ++i)
    {
        int index = array[i];
        if(m_graph.m_nodes[index].GetID() == -1)
            continue;
        D3DXVECTOR3 temp = position-m_graph.m_nodes[index].GetPos();
        if(temp.x == 0 && temp.y == 0 && temp.z == 0)
            temp.x = 1;
        float sqrDist = D3DXVec3LengthSq(&temp);
        if(sqrDist < minDistSquared)
        {
            minDistSquared = sqrDist;
            currentIndex = index;
        }
    }
    return currentIndex;
}
void Grid::DrawNode( int index, const D3DXVECTOR3& color ) const
{
    Engine::Instance()->GetLineDrawer()->drawLine(m_graph.Nodes()[index].GetPos(),
        m_graph.Nodes()[index].GetPos() + m_nodeDrawDirection,
        color);
}

